/***********************************************************************
 * $Id: fpu_enable.c
 *
 * Project: LPC43xx
 *
 * Description: fpu initialization routine
 *
 * Copyright(C) 2011, NXP Semiconductor
 * All rights reserved.
 *
 ***********************************************************************
 * Software that is described herein is for illustrative purposes only
 * which provides customers with programming information regarding the
 * products. This software is supplied "AS IS" without any warranties.
 * NXP Semiconductors assumes no responsibility or liability for the
 * use of the software, conveys no license or title under any patent,
 * copyright, or mask work right to the product. NXP Semiconductors
 * reserves the right to make changes in the software without
 * notification. NXP Semiconductors also make no representation or
 * warranty that such application will be suitable for the specified
 * use without further testing or modification.
 **********************************************************************/

#define    LPC_CPACR            0xE000ED88

#define SCB_MVFR0           0xE000EF40
#define SCB_MVFR0_RESET     0x10110021

#define SCB_MVFR1           0xE000EF44
#define SCB_MVFR1_RESET     0x11000011

#include "stdint.h"

void fpuEnable(void)
{
    /* from arm trm manual, howto enable the FPU :
        ; CPACR is located at address 0xE000ED88
        LDR.W R0, =0xE000ED88
        ; Read CPACR
        LDR R1, [R0]
        ; Set bits 20-23 to enable CP10 and CP11 coprocessors
        ORR R1, R1, #(0xF << 20)
        ; Write back the modified value to the CPACR
        STR R1, [R0]
    */
    volatile uint32_t *regCpacr = (uint32_t *) LPC_CPACR;
    volatile uint32_t *regMvfr0 = (uint32_t *) SCB_MVFR0;
    volatile uint32_t *regMvfr1 = (uint32_t *) SCB_MVFR1;
    volatile uint32_t Cpacr;
    volatile uint32_t Mvfr0;
    volatile uint32_t Mvfr1;
    char vfpPresent = 0;
    Mvfr0 = *regMvfr0;
    Mvfr1 = *regMvfr1;
    vfpPresent = ((SCB_MVFR0_RESET == Mvfr0) && (SCB_MVFR1_RESET == Mvfr1));

    /* enable the FPU if present on target */
    if (vfpPresent) {
        Cpacr = *regCpacr;
        Cpacr |= (0xF << 20);
        *regCpacr = Cpacr;   // enable CP10 and CP11 for full access
    }
}


